#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_watchdog.h"
#include "robot_control.h"
#include "iot_errno.h"
#include "hi_pwm.h"
#include "hi_timer.h"
#include "iot_pwm.h"
#include <stdio.h>
#include <stdlib.h>
//uart部分头
#include <unistd.h>
#include <string.h>

#include "iot_gpio_ex.h"
#include "iot_uart.h"
#include "hi_uart.h"

//左右两轮电机各由一个L9110S驱动
//GPOI0和GPIO1控制左轮,GPIO9和GPIO10控制右轮。通过输入GPIO的电平高低控制车轮正转/反转/停止/刹车。
#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10

//查阅机器人板原理图可知
//左边的红外传感器通过GPIO11与3861芯片连接
//右边的红外传感器通过GPIO12与3861芯片连接
#define GPIO11 11
#define GPIO12 12

#define GPIO_FUNC 0
#define car_speed_left 0
#define car_speed_right 0
extern unsigned char g_car_status;   
extern void car_stop(void);
extern void car_backward(void);
extern void car_forward(void);
extern void car_left(void);
extern void car_right(void);
unsigned int g_car_speed_left = car_speed_left;
unsigned int g_car_speed_right = car_speed_right;
IotGpioValue io_status_left;
IotGpioValue io_status_right;
//uart部分函数
#define UART_BUFF_SIZE 100
#define U_SLEEP_TIME   100000

void Uart1GpioInit(void)
{
    IoTGpioInit(IOT_IO_NAME_GPIO_5);
    IoSetFunc(IOT_IO_NAME_GPIO_5, IOT_IO_FUNC_GPIO_5_UART1_RXD);
    IoTGpioInit(IOT_IO_NAME_GPIO_6);
    IoSetFunc(IOT_IO_NAME_GPIO_6, IOT_IO_FUNC_GPIO_6_UART1_TXD);
}

void Uart1Config(void)
{
    uint32_t ret;
    /* 初始化UART配置，波特率 115200，数据bit为8,停止位1，奇偶校验为NONE */
    IotUartAttribute uart_attr = {
        .baudRate = 115200,
        .dataBits = 8,
        .stopBits = 1,
        .parity = 0,
    };
    ret = IoTUartInit(HI_UART_IDX_1, &uart_attr);
    if (ret != IOT_SUCCESS) {
        printf("Init Uart1 Falied Error No : %d\n", ret);
        return;
    }
}

static int UartTask(void)
{
    int a[4];
    uint32_t len = 0;
    unsigned char uartReadBuff[UART_BUFF_SIZE] = {0};
    uint32_t count = 0;
    // 对UART1的一些初始化
    Uart1GpioInit();
    // 对UART1参数的一些配置
    Uart1Config();

    
    // 通过UART1 接收数据
    len = IoTUartRead(HI_UART_IDX_1, uartReadBuff, UART_BUFF_SIZE);
    if (len > 0) {
            // 把接收到的数据打印出来
        printf("Uart Read Data is: [ %d ] %s \r\n", count, uartReadBuff);
            //count是收到的第几组数据
     }
        usleep(U_SLEEP_TIME);
        count++;
    sscanf((const char*)uartReadBuff, "xmin:%d ,ymin:%d ,xmax:%d , ymax:%d", &a[0], &a[1], &a[2], &a[3]);//提取出数字部分并存到数组中

    return (a[0] + a[2]) / 2 + 1;
}



//获取红外传感器的值，调整电机的状态
void timer1_callback(int xmid)
{
    //小车往左偏，则需要向右修正方向(右转)
    if(xmid >= 1020)
    {   
        //则GPIO1输出低电平,GPIO10输出高电平。小车右转（右轮不转，左轮正转）,高电平为不转
        g_car_speed_left = car_speed_left;
        g_car_speed_right = 1;
    } 

    //小车往右偏，则需要向左修正方向（左转）
    if(xmid <= 900)
    {
        //则GPIO1输出高电平,GPIO10输出低电平。小车左转（右轮正转，左轮不转）
        g_car_speed_left = 1;
        g_car_speed_right = car_speed_right;
    }
   //hi_udelay(50);
   printf("xmid:%d\n",xmid);
    
}

void trace_module(void)
{  
    int xmid = UartTask();
    int i=0;
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, g_car_speed_left);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, g_car_speed_right);
    hi_sleep(100);
    
        if(xmid >= 1020)
        {
            car_right();
            hi_sleep(500);
            car_stop();
        }
        else if(xmid<=900)
        {
            car_left();
            hi_sleep(500);
            car_stop();
        }
        else{
            car_forward();
            hi_sleep(500);
        }
        hi_sleep(100);
    
    }







}
